#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import rospy
import cv2
import os
import sys
import glob
import numpy as np
import math
import time

from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge


#距离映射
x_cmPerPixel = 90/665.00
y_cmPerPixel = 81/680.00
roadWidth = 665

y_offset = 50.0 #cm

#轴间距
I = 58.0
#摄像头坐标系与车中心间距
D = 18.0
#计算cmdSteer的系数
k = -10

fourcc = cv2.VideoWriter_fourcc('X', 'V', 'I', 'D')
videoNameString = time.strftime("%Y%m%d-%H%M%S_video.avi", time.localtime())
class camera:
    def __init__(self):

        self.camMat = []
        self.camDistortion = []

        self.cap = cv2.VideoCapture('/dev/video10')
        # self.cap = cv2.VideoCapture('video2.avi')
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)

        self.vout = cv2.VideoWriter()
        self.vout.open(videoNameString, fourcc, 10, (1280, 720), True)

        self.imagePub = rospy.Publisher('images', Image, queue_size=1)
        self.cmdPub = rospy.Publisher('lane_vel', Twist, queue_size=1)
        self.cam_cmd = Twist()
        self.cvb = CvBridge()
        
        # src_points = np.array([[3,570], [387,460], [906,452], [1041,485]], dtype="float32")
        # dst_points = np.array([[266., 686.], [266., 19.], [931., 20.], [931., 701.]], dtype="float32")
        self.leftdown = [50,460]
        self.leftup = [360,380]
        self.rightup = [925,405]
        self.rightdown = [1205,485]
        
        self.leftdown_dst = [307, 654]
        self.leftup_dst = [307, 66]
        self.rightup_dst = [972, 66]
        self.rightdown_dst = [972, 654]
        src_points = np.array([self.leftdown, self.leftup, self.rightup, self.rightdown], dtype="float32")
        dst_points = np.array([self.leftdown_dst, self.leftup_dst, self.rightup_dst, self.rightdown_dst], dtype="float32")
        ## 透视矩阵
        self.M = cv2.getPerspectiveTransform(src_points, dst_points)  

        self.aP = [0.0, 0.0]
        self.lastP = [0.0, 0.0]
        self.Timer = 0
    
    def __del__(self):
        self.cap.release()
        self.vout.release()

    def spin(self):
        ret, img = self.cap.read()
        if ret == True:
	      #  print('1')
            # self.vout.write(img)
            gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
            kernel = np.ones((3,3), np.uint8)
            gray_img = cv2.erode(gray_img, kernel, iterations=1) #腐蚀操作
            # gray_img = cv2.morphologyEx(gray_img, cv2.MORPH_OPEN,kernel) #开运算
        
            origin_thr = np.zeros_like(gray_img)
            origin_thr[(gray_img >= 125)] = 255
            
            binary_warped = cv2.cvtColor(origin_thr, cv2.COLOR_GRAY2BGR)  
            #cv2.circle(binary_warped, (self.leftdown[0],self.leftdown[1]), 10, (0, 0, 255), -1) # 画出预瞄点
           # cv2.circle(binary_warped, (self.leftup[0],self.leftup[1]), 10, (0, 0, 255), -1) # 画出预瞄点
            #cv2.circle(binary_warped, (self.rightup[0],self.rightup[1]), 10, (0, 0, 255), -1) # 画出预瞄点
           # cv2.circle(binary_warped, (self.rightdown[0],self.rightdown[1]), 10, (0, 0, 255), -1) # 画出预瞄点       
            
            
            
            binary_warped = cv2.warpPerspective(binary_warped, self.M, (1280, 720), cv2.INTER_LINEAR) 
            cv2.circle(binary_warped, (self.leftdown_dst[0],self.leftdown_dst[1]), 30, (0, 0, 255), -1) # 画出预瞄点
            cv2.circle(binary_warped, (self.leftup_dst[0],self.leftup_dst[1]), 30, (0, 0, 255), -1) # 画出预瞄点
            cv2.circle(binary_warped, (self.rightup_dst[0],self.rightup_dst[1]), 30, (0, 0, 255), -1) # 画出预瞄点
            cv2.circle(binary_warped, (self.rightdown_dst[0],self.rightdown_dst[1]), 30, (0, 0, 255), -1) # 画出预瞄点
            
            cv2.imshow('origin_thr',binary_warped)
            #self.cmdPub.publish(0)
            cv2.waitKey(1)



if __name__ == '__main__':
    rospy.init_node('lane_vel', anonymous=True)
    rate = rospy.Rate(10)
    
    try:
        cam = camera()
        print(rospy.is_shutdown())  # FALSE
        while not rospy.is_shutdown():
            cam.spin()
            #print('betweeen == cam.spin ==')
            rate.sleep()
    except rospy.ROSInterruptException:
        print(rospy.ROSInterruptException)
        pass


